#include "PathSearch.h"

namespace fullsail_ai { namespace algorithms {

	PathSearch::PathSearch() : goal(0), tileMap(0), goalFound(false), open(CheckDistance)
	{
	}

	PathSearch::~PathSearch()
	{
		exit();
	}

	void PathSearch::initialize(TileMap* _tileMap)
	{
		tileMap = _tileMap;
	}

	void PathSearch::enter(int _startRow, int _startColumn, int _goalRow, int _goalColumn)
	{
		Tile* start = tileMap->getTile(_startRow, _startColumn);
		goal = tileMap->getTile(_goalRow, _goalColumn);
		open.push(new PlannerNode(start,0, DistanceBetweenTiles(start,goal) * HEURISTIC_WEIGHT));
		created[open.front()->tile] = open.front();
		goalFound = false;
	}

	void PathSearch::DrawPath( PlannerNode * const path)
	{
		PlannerNode* lineNode = path;

		while(lineNode->parent)
		{
			lineNode->parent->tile->setLineTo(lineNode->tile, RED);
			lineNode->parent->tile->setOutline(WHITE);
			lineNode = lineNode->parent;
		}
	}

	void PathSearch::update(long timeslice)
	{
		DWORD timeElapsed;
		DWORD startTime = GetTickCount();

		do
		{
			if(open.size())
			{
				PlannerNode* current = open.front();
				open.pop();

				if(current->tile == goal)
				{
					tileMap->resetTileDrawing();
					goalFound = true;

					PlannerNode* iter = current;

					while(iter)
					{
						solution.push_back(iter->tile);
						iter = iter->parent;
					}

					DrawCreated();
					DrawPath(current);

					return;
				}

				if(!timeslice)
				{
					tileMap->resetTileDrawing();
					DrawCreated();
					DrawPath(current);
				}

				int row = current->tile->getRow();
				int col = current->tile->getColumn();

				EvaluateTile(row+1, col+(1 - (2 * (~row&1))),current);
				EvaluateTile(row-1, col+(1 - (2 * (~row&1))),current);
				EvaluateTile(row + 1, col,current);
				EvaluateTile(row, col+1,current);
				EvaluateTile(row, col-1,current);
				EvaluateTile(row-1, col,current);
			}

			timeElapsed = GetTickCount() - startTime;
		}while(timeElapsed < (DWORD)timeslice);
	}

	void PathSearch::exit()
	{
		ClearCreated();
		solution.clear();
		open.clear();
	}

	void PathSearch::shutdown()
	{
		exit();
	}

	bool PathSearch::isDone() const
	{
		return goalFound;
	}

	std::vector<Tile const*> const PathSearch::getSolution() const
	{
		return solution;
	}

	void PathSearch::EvaluateTile(int row, int col, PlannerNode * cur)
	{
		if(row < tileMap->getRowCount() && row >= 0 && col < tileMap->getColumnCount() && col >= 0)
		{
			Tile* successor = tileMap->getTile(row, col);

			if(successor->getWeight())
			{
				successor->setOutline(YELLOW);
				successor->setMarker(GREEN);

				double tempCost = cur->givenCost + DistanceBetweenTiles(successor,cur->tile) * cur->tile->getWeight();
				
				PlannerNode* &temp = created[successor];

				if(temp)
				{
					if(temp->givenCost > tempCost)
					{
						open.remove(temp);
						temp->parent = cur;
						temp->givenCost = tempCost;
						temp->finalCost = tempCost + temp->heuristicCost;
						open.push(temp);
					}					
				}
				else
				{
					temp = new PlannerNode(successor,tempCost,DistanceBetweenTiles(successor,goal)*HEURISTIC_WEIGHT);
					temp->finalCost = tempCost + DistanceBetweenTiles(successor,goal)*HEURISTIC_WEIGHT;
					temp->parent = cur;
					open.push(temp);
					successor->setFill(GREEN);
				}
			}
		}
	}

	void PathSearch::DrawCreated()
	{
		for(std::map<Tile*, PlannerNode*>::iterator iter = created.begin(); iter != created.end(); ++iter)
		{
			iter->first->setFill(BLUE);
		}
	}

	void PathSearch::ClearCreated()
	{
		if(tileMap)
		{
			for(std::map<Tile*, PlannerNode*>::iterator iter = created.begin(); iter != created.end(); ++iter)
			{
				PlannerNode* temp = iter->second;
				delete temp;
			}

			created.clear();
		}
	}

	double PathSearch::DistanceBetweenTiles(Tile const* a, Tile const* b)
	{
		double x = b->getXCoordinate() - a->getXCoordinate();
		double y = b->getYCoordinate() - a->getYCoordinate();

		return sqrt((x*x) + (y*y));
	}
}}  // namespace fullsail_ai::algorithms

